#!/usr/bin/env python

import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import smach
import smach_ros
import time

from library_highlevel            import library_highlevel
from state_initialise             import Initialise
from state_stop                   import Stop
from state_goToDepth              import GoToDepth
from state_actionserver_goToDepth import GoToDepthServer
from state_goToWaypoint           import GoToWaypoint
from DelphinROSv2.msg             import compass
from DelphinROSv2.msg             import GoToDepthAction, GoToDepthGoal
from actionlib                    import *
from actionlib.msg                import *


            
def main():
    rospy.init_node('smach_example_state_machine')
    
    # Define an instance of highlevelcontrollibrary to pass to all action servers
    lib = library_highlevel()
    
    # ...and an action server for the GoToDepth action
    #server = GoToDepthServer('test_depth_action', lib, False)



    # Create a SMACH state machine - with outcome 'finish'
    sm = smach.StateMachine(outcomes=['finish'])

    # Open the container
    with sm:
        # Add states to the container
        # generic state
        smach.StateMachine.add('INITIALISE', Initialise(lib,15), #15 = timeout for initialisation state
            transitions={'succeeded':'SUBMERGE', 'aborted':'STOP','preempted':'STOP'})
            
        # client side of action server - specifies the goal conditions and outcomes (i.e. which state to transition to on failure, on success, on pre-empt..
       # smach.StateMachine.add('SUBMERGE', 
        #    smach_ros.SimpleActionState('test_depth_action', GoToDepthAction,  goal = GoToDepthGoal(depthDemand=10, timeout=20, tolerance=1)),
        #    {'succeeded':'GO_TO_WAYPOINT', 'aborted':'STOP', 'preempted':'STOP'})
        
        smach.StateMachine.add('SUBMERGE', GoToDepth(lib, 1, 0.25, 60), 
            transitions={'succeeded':'GO_TO_WAYPOINT', 'aborted':'STOP','preempted':'STOP'})

        # generic states
        smach.StateMachine.add('GO_TO_WAYPOINT', GoToWaypoint(lib), 
            transitions={'succeeded':'STOP', 'aborted':'STOP','preempted':'STOP'})
        smach.StateMachine.add('STOP', Stop(lib), 
            transitions={'succeeded':'finish'})        
     
   

    # Create and start the introspection server - for visualisation
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
    sis.start()
    
    

    # Execute the state machine
    outcome = sm.execute()
    print 'finished executing state machine'

    # Wait for ctrl-c to stop the application
    rospy.spin()
    sis.stop()




if __name__ == '__main__':
    main()
